/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *	Robot's Link Library.
 *	Holds the DH's parameters of each Link and the transformations of each Link.
 *
 *   Authors:	Murilo Marques Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

#include<stdlib.h>
#include<stdio.h>

#include<schunk_high/link.h>
#include<schunk_high/dualquaternion.h>

#include<math.h>

/**
Allocates a new Link and initializes it using the given DH parameters.

@param theta The link's DH's theta
@param d     The Link's DH's d
@param a     The Link's DH's a
@param alpha The Link's DH's alpha
@return A Link with the given DH parameters.
*/
Link* Link_create(double theta, double d, double a, double alpha){
	
	Link* l = (Link*)malloc(sizeof(Link));
	l->theta = theta;
	l->d     = d;
	l->a     = a;
	l->alpha = alpha;
	l->jvel  = 0;
	l->n = NULL;
	
	return l;
}

/**
Frees a Link from memory.

@param l The Link to be freed.
*/
void Link_free(Link* l){
	free(l);
}


/**
DualQuaternion Link Frame Transformation.

@param l The Link which DualQuaternion Frame Transformation you want.
@return A DualQuaternion representing it's transformation.
*/
DQ* Link_DQFT(Link* l){

	//Link's values
	double theta = l->theta;
	double d = l->d;
	double a = l->a;
	double alpha = l->alpha;

	//Used values
	double ct2 = cos(theta/2); double st2 = sin(theta/2);
	double ca2 = cos(alpha/2); double sa2 = sin(alpha/2);
	double d2 = d/2;           double a2 = a/2;

	//Memory Alloc	
	DQ* dq = DQ_alloc();
	Q* pp = Q_alloc();
	Q* dp = Q_alloc();
	
	//Calculation
    pp->v[0] = ct2*ca2;
	pp->v[1] = ct2*sa2;
	pp->v[2] = st2*sa2;
	pp->v[3] = st2*ca2;
	dp->v[0] = (-d2)*pp->v[3] - (a2)*pp->v[1];
	dp->v[1] = (-d2)*pp->v[2] + (a2)*pp->v[0];
	dp->v[2] =  (d2)*pp->v[1] + (a2)*pp->v[3];
	dp->v[3] =  (d2)*pp->v[0] - (a2)*pp->v[2];
	
	//Atrib
	dq->p = pp;
	dq->d = dp;
	
	return dq;
}

